import json
import matplotlib.pyplot as plt
from shapely.geometry import Polygon
from pyproj import Transformer
import math
from PIL import Image
import numpy as np

# 经纬度转米（Web Mercator）
transformer = Transformer.from_crs("EPSG:4326", "EPSG:3857", always_xy=True)

# 读取 JSON
with open(r'D:\files\农业机器人任务规划研究\ros_workspace\果园路网\东园\wgs84.json', 'r', encoding='utf-8') as f:
    data = json.load(f)

rectangles = []
all_x = []
all_y = []

# 构造矩形
for edge in data["edges"]:
    lon1, lat1 = edge["sLongitude"], edge["sLatitude"]
    lon2, lat2 = edge["eLongitude"], edge["eLatitude"]

    x1, y1 = transformer.transform(lon1, lat1)
    x2, y2 = transformer.transform(lon2, lat2)

    dx = x2 - x1
    dy = y2 - y1
    length = math.hypot(dx, dy)

    nx = -dy / length
    ny = dx / length

    offset = 1.5  # 1 米

    p1 = (x1 + nx * offset, y1 + ny * offset)
    p2 = (x1 - nx * offset, y1 - ny * offset)
    p3 = (x2 - nx * offset, y2 - ny * offset)
    p4 = (x2 + nx * offset, y2 + ny * offset)

    rect = Polygon([p1, p2, p3, p4])
    rectangles.append(rect)

    for px, py in [p1, p2, p3, p4]:
        all_x.append(px)
        all_y.append(py)

# 边界计算（加 5 米 margin）
margin = 5.0
min_x = min(all_x) - margin
max_x = max(all_x) + margin
min_y = min(all_y) - margin
max_y = max(all_y) + margin

# 分辨率：1 像素 = 0.05 米
resolution = 0.05  # 米/像素
width_m = max_x - min_x
height_m = max_y - min_y
width_px = int(width_m / resolution)
height_px = int(height_m / resolution)

# 初始化黑色图像 (白为255, 黑为0)
img = np.zeros((height_px, width_px), dtype=np.uint8)

# 坐标转换函数：米 -> 图像像素坐标（左上角为原点）
def to_pixel_coords(x, y):
    px = int((x - min_x) / resolution)
    py = int((max_y - y) / resolution)  # 图像坐标 y 轴向下
    return px, py

# 绘制每个矩形到图像中（白色填充）
from PIL import ImageDraw

# 创建 PIL Image 对象（灰度）
pil_img = Image.fromarray(img, mode='L')
draw = ImageDraw.Draw(pil_img)

for rect in rectangles:
    coords = [to_pixel_coords(x, y) for x, y in rect.exterior.coords]
    draw.polygon(coords, fill=255)

# 保存 PNG 文件
pil_img.save("edge_rectangles_scaled.png")

# 保存 PGM 文件（P2 ASCII 格式）
pil_img.save("edge_rectangles_scaled.pgm")

# ✅ 在此处输入你的经纬度点
ref_longitude = 118.11490305
ref_latitude = 35.54194928

# 将经纬度点转换为米坐标
ref_x, ref_y = transformer.transform(ref_longitude, ref_latitude)

# 计算该点距离图像左边界（min_x）和下边界（min_y）的距离
x_offset_m = ref_x - min_x  # 距左边界距离
y_offset_m = ref_y - min_y  # 距下边界距离

print(f"经纬度点 ({ref_latitude}, {ref_longitude})：")
print(f"→ 距离图像左边界：{- x_offset_m:.2f} 米")
print(f"→ 距离图像下边界：{- y_offset_m:.2f} 米")

# 计算地图中心点（米）
center_x = (min_x + max_x) / 2
center_y = (min_y + max_y) / 2

# 给定点 ref_x, ref_y 已在前面转换完成

# 计算中心点到该点的水平/竖直距离（米）
horizontal_offset = ref_x - center_x
vertical_offset = ref_y - center_y

print(f"\n地图中心点为 (x: {center_x:.2f}, y: {center_y:.2f})")
print(f"→ 给定点相对中心点的水平偏移: {horizontal_offset:.2f} 米")
print(f"→ 给定点相对中心点的竖直偏移: {vertical_offset:.2f} 米")


# import random
# from shapely.geometry import Point

# # 创建反向投影器（米 → 经纬度）
# inverse_transformer = Transformer.from_crs("EPSG:3857", "EPSG:4326", always_xy=True)

# # 从一个矩形中选取
# selected_rect = random.choice(rectangles)

# # 获取矩形中心点（米坐标）
# centroid_point = selected_rect.centroid
# centroid_x, centroid_y = centroid_point.x, centroid_point.y

# # 转换为经纬度
# centroid_lon, centroid_lat = inverse_transformer.transform(centroid_x, centroid_y)

# print(f"✅ 白色矩形中点为：")
# print(f"经度: {centroid_lon:.6f}, 纬度: {centroid_lat:.6f}")

